#include "MCMatrix3.h"
#include <MCVector2.h>
#include <MCMathUtils.h>
#include <MCPoint2D.h>
#include <iostream>

MCMatrix3::MCMatrix3() :
    m11(0), m12(0), m13(0),
    m21(0), m22(0), m23(0),
    m31(0), m32(0), m33(0)
{
    
}

MCMatrix3::MCMatrix3(double m11_, double m12_, double m13_, double m21_, double m22_, double m23_, double m31_, double m32_, double m33_) :
    m11(m11_), m12(m12_), m13(m13_),
    m21(m21_), m22(m22_), m23(m23_),
    m31(m31_), m32(m32_), m33(m33_)
{
}

MCMatrix3 MCMatrix3::identity()
{
    return MCMatrix3(
        1, 0, 0,
        0, 1, 0,
        0, 0, 1
    );
}

MCMatrix3 MCMatrix3::translate(const MCVector2 &v)
{
    return MCMatrix3(
        1.0, 0.0, v.x,
        0.0, 1.0, v.y,
        0.0, 0.0, 1.0);
}

MCMatrix3 MCMatrix3::scale(double sx, double sy)
{
    return MCMatrix3(
        sx, 0, 0,
        0, sy, 0,
        0,  0, 1
    );
}

MCMatrix3 MCMatrix3::rotateInRadian(double angle)
{
    double cos_ = std::cos(angle);
    double sin_ = std::sin(angle);

    if (std::abs(cos_) < 1e-16) cos_ = 0;
    if (std::abs(sin_) < 1e-16) sin_ = 0;

    return MCMatrix3(
        cos_, -sin_, 0.0,
        sin_, cos_, 0.0,
        0.0, 0.0, 1.0);
}

MCMatrix3 MCMatrix3::rotateInDegrees(double angle)
{
    return rotateInRadian(MCMathUtils::degreeToRadian(angle));
}

void MCMatrix3::print() const
{
    std::cout << m11 << " " << m12 << " " << m13 << "\n";
    std::cout << m21 << " " << m22 << " " << m23 << "\n";
    std::cout << m31 << " " << m12 << " " << m33 << "\n";
}

void operator+=(MCMatrix3 &m, double scalar)
{
    m.m11 += scalar; m.m12 += scalar; m.m13 += scalar;
    m.m21 += scalar; m.m22 += scalar; m.m23 += scalar;
    m.m31 += scalar; m.m32 += scalar; m.m33 += scalar;
}

void operator*=(MCMatrix3 &m, double scalar)
{
    m.m11 *= scalar; m.m12 *= scalar; m.m13 *= scalar;
    m.m21 *= scalar; m.m22 *= scalar; m.m23 *= scalar;
    m.m31 *= scalar; m.m32 *= scalar; m.m33 *= scalar;
}

void operator+=(MCMatrix3 &m1, MCMatrix3 &m2)
{
    m1.m11 += m2.m11; m1.m12 += m2.m12; m1.m13 += m2.m13;
    m1.m21 += m2.m21; m1.m22 += m2.m22; m1.m23 += m2.m23;
    m1.m31 += m2.m31; m1.m32 += m2.m32; m1.m33 += m2.m33;
}

void operator-=(MCMatrix3 &m1, MCMatrix3 &m2)
{
    m1.m11 -= m2.m11; m1.m12 -= m2.m12; m1.m13 -= m2.m13;
    m1.m21 -= m2.m21; m1.m22 -= m2.m22; m1.m23 -= m2.m23;
    m1.m31 -= m2.m31; m1.m32 -= m2.m32; m1.m33 -= m2.m33;
}

MCPoint2D operator*(const MCPoint2D &p, const MCMatrix3 &m)
{
    return MCPoint2D(
        m.m11 * p.x + m.m12 * p.y + m.m13,
        m.m21 * p.x + m.m22 * p.y + m.m23);
}
